#include "bflb_mtimer.h"        //mtimer定时器头文件
#include "bflb_pwm_v2.h"        //pwm_v2头文件
#include "bflb_clock.h"         //系统时钟头文件
#include "board.h"              //开发板头文件，包装的库
#include "bflb_gpio.h"          //gpio头文件
#include "bflb_servo.h"


struct bflb_servo_attr bflb_servo_attach(uint8_t pin, uint8_t ch){

    struct bflb_device_s *pwm;     //创建LHAL外设库结构体，名称为pwm
    struct bflb_device_s *gpio;    //初始化一个结构体指针作为外设句柄
    struct bflb_servo_attr servo_attr; // 创建要返回的结构体实例

    gpio = bflb_device_get_by_name("gpio");

    bflb_gpio_init(gpio, pin, GPIO_FUNC_PWM0 | GPIO_ALTERNATE | GPIO_PULLUP | GPIO_SMT_EN | GPIO_DRV_1);
    pwm = bflb_device_get_by_name("pwm_v2_0");  //给外设接口赋名pwm_v2_0

    /* period = .XCLK / .clk_div / .period = 40MHz / 16000 / 50 = 50Hz */
    struct bflb_pwm_v2_config_s cfg = {
        .clk_source = BFLB_SYSTEM_XCLK,
        .clk_div = 160,
        .period = 5000,
    };                //设置PWM的频率，选择时钟，分频，和周期。根据上面的公式算出最终的频率。


    /*初始化PWM输出*/
    bflb_pwm_v2_init(pwm, &cfg);

    // bflb_pwm_v2_channel_positive_start 开启正向通道(CHxP)输出
    // bflb_pwm_v2_channel_negative_start开启负向通道(CHxN) 从参数内可以查询

    bflb_pwm_v2_channel_positive_start(pwm, ch);  //IO19是正向通道3
    bflb_pwm_v2_start(pwm);  //将设置好的频率开启pwm输出

    servo_attr.dev = pwm;  
    servo_attr.pin = pin;  
    servo_attr.ch = ch;  
  
    return servo_attr;
}


int angle_to_pwm(int angle) {  
    if (angle < SERVO_ANGLE_MIN) angle = SERVO_ANGLE_MIN;  
    if (angle > SERVO_ANGLE_MAX) angle = SERVO_ANGLE_MAX;  
      
    // 线性插值计算PWM值  
    int range = SERVO_ANGLE_MAX - SERVO_ANGLE_MIN;  
    int pwm_range = PWM_MAX - PWM_MIN;  
    int pwm_value = PWM_MIN + ((angle - SERVO_ANGLE_MIN) * pwm_range / range);  
      
    return pwm_value;  
}

// 写入伺服角度值（以度为单位）  
void bflb_servo_write(struct bflb_servo_attr *servo, int angle) {

    // 将角度转换为PWM占空比  
    int pwm_value = angle_to_pwm(angle);
    // 调用pwm修改占空比
    bflb_pwm_v2_channel_set_threshold(servo->dev, servo->ch, 0, pwm_value);
}

void bflb_servo_stop(struct bflb_servo_attr *servo){
    // 停止PWM信号发生
    bflb_pwm_v2_channel_positive_stop(servo->dev, servo->ch);
}